#include "fishbot_multipoint_nav/fishbot_multipoint_nav.h"
void GoalSender::grayStateCallBack(const std_msgs::msg::Int16::SharedPtr msg)
{

  int gray_state = msg->data;

  if (gray_state == 0)
  {
    param_manager_->reloadParameters("/home/fwy/nav_ws/src/fishbot_setparam/config/new_param.yaml");
  }

  if (gray_state == 1)
  {
    amcl_param_updater_node_->update(0.5);
  }

  if (gray_state == 0)
  {
    amcl_param_updater_node_->update(0.6);
  }

}
int main(int argc, char ** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<GoalSender>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}